#include <assert.h>
#include <memory.h>
#include "circle_queue.h"

#define INITIAL_QUEUE_SIZE 16


static inline void *getPointer(circle_queue_t *cir_queue, uint16_t index)
{
    return ((uint8_t *)cir_queue->queue) + index * cir_queue->element_size;
}


static inline bool isQueueEmpty(circle_queue_t *cir_queue)
{
    return cir_queue->front == cir_queue->rear;
}


static inline bool isQueueFull(circle_queue_t *cir_queue)
{
    return (cir_queue->rear + 1 + 1 /* One more element for function GetUpdatedHead */) % cir_queue->size == cir_queue->front;
}


static inline bool doubleQueueSize(circle_queue_t *cir_queue)
{
    uint32_t new_size = 0;
    void *new_queue = NULL;

    if (cir_queue->size == MAX_QUEUE_SIZE) {
        return false;
    }

    new_size = cir_queue->size * 2;
    new_queue = malloc(cir_queue->element_size * new_size);
    if (new_queue == NULL) {
        return false;
    }

    memcpy(new_queue, cir_queue->queue, cir_queue->element_size * cir_queue->rear);
    if (cir_queue->front > cir_queue->rear) {
        memcpy(
            (uint8_t *)new_queue + cir_queue->element_size * (cir_queue->size + cir_queue->front - 1 /* One more element for function GetUpdatedHead */),
            getPointer(cir_queue, cir_queue->front - 1 /* One more element for function GetUpdatedHead */),
            (cir_queue->size - cir_queue->front + 1 /* One more element for function GetUpdatedHead */) * cir_queue->element_size
        );
        cir_queue->front += cir_queue->size;
    } else {
        if (cir_queue->front == 0) {    /* Copy one more element for function GetUpdatedHead */
            memcpy(
                (uint8_t *)new_queue + cir_queue->element_size * (new_size - 1),
                getPointer(cir_queue, cir_queue->size - 1),
                cir_queue->element_size
            );
        }
    }
    free(cir_queue->queue);
    cir_queue->queue = new_queue;
    cir_queue->size = new_size;

    return true;
}


circle_queue_t *CircleQueue_NewQueue(const size_t element_size)
{
    assert(element_size != 0);

    void *queue = NULL;
    circle_queue_t *circle_queue = NULL;

    queue = malloc(element_size * INITIAL_QUEUE_SIZE);
    if (queue == NULL) {
        return NULL;
    }

    circle_queue = malloc(sizeof(circle_queue_t));
    if (circle_queue == NULL) {
        free(queue);
        return NULL;
    }

    circle_queue->element_size = element_size;
    circle_queue->front = 0;
    circle_queue->rear = 0;
    circle_queue->queue = queue;
    circle_queue->size = INITIAL_QUEUE_SIZE;

    return circle_queue;
}


void CircleQueue_Destroy(circle_queue_t *cir_queue)
{
    assert(cir_queue != NULL);
    assert(cir_queue->queue != NULL);

    free(cir_queue->queue);
    cir_queue->queue = NULL;
    free(cir_queue);
}


void *CircleQueue_Dequeue(circle_queue_t *cir_queue)
{
    assert(cir_queue != NULL);
    assert(cir_queue->queue != NULL);

    void *dequeue_element = NULL;

    if (isQueueEmpty(cir_queue)) {
        return NULL;
    }

    dequeue_element = getPointer(cir_queue, cir_queue->front);
    cir_queue->front = (cir_queue->front + 1) % cir_queue->size;

    return dequeue_element;
}


bool CircleQueue_Enqueue(circle_queue_t *cir_queue, void *element)
{
    assert(cir_queue != NULL);
    assert(cir_queue->queue != NULL);
    assert(element != NULL);

    void *enqueue_element = NULL;

    if (isQueueFull(cir_queue) && !doubleQueueSize(cir_queue)) {
        return false;
    }

    enqueue_element = getPointer(cir_queue, cir_queue->rear);
    memcpy(enqueue_element, element, cir_queue->element_size);

    cir_queue->rear = (cir_queue->rear + 1) % cir_queue->size;

    return true;
}


/**
    @brief Get the updated head node pointer.

    After queue just done double its size, any old pointer to
    any node in old queue was no longer valid. If the caller (of dequeue)
    has not save the data, then it can use this function to update
    the pointer.

    @note To use this function correctly, function `doubleQueueSize`
    needs to save one more node while doubling.
 */
void *CircleQueue_GetUpdatedHead(circle_queue_t *cir_queue)
{
    assert(cir_queue != NULL);
    assert(cir_queue->queue != NULL);

    uint16_t old_front = cir_queue->front == 0 ?
                         cir_queue->size - 1 :
                         cir_queue->front - 1;

    return getPointer(cir_queue, old_front);
}